#ifndef __TAIL_CONTROL_H__
#define __TAIL_CONTROL_H__
#include <string>
#include <thread>

#include "com_ctrl.h"
#include "tail_motor_data.h"
#include "motor_ctrl.h"

/* 通讯构造参数描述 */
typedef struct {    
    uint8_t len;
    uint32_t param[10];
    Com_Type type;
} Com_Desc, *pCom_Desc;

/* 电机构造参数描述 */
typedef struct {    
    uint8_t motor_n;
    Motor_ID_Info motor_array[MOTOR_NUM_MAX];
} Motor_Desc, *pMotor_Desc;

class TailControl {
public:
    TailControl(const std::string& filename, int hz) : filename(filename), hz(hz) { }
    void tailInit();
    void tailLoop();

    int excelDataRead();
    int zeroPosSet();
    int comInit(Com_Desc &com_param);
    int motorInit(Motor_Desc &motor_param);

    void read();
    void update();
    void write();

    void calculatePosCommand(double& joint_command, uint8_t num);
    void calculateTauCommand(double& joint_command, uint8_t num);
    void calculateTailPos(double& joint_command, uint8_t num);
    void posLimit();

    Com_Ctrl *p_com_ctrl = nullptr;
    Motor_Ctrl  *p_motor_ctrl   = nullptr;

    pthread_mutex_t mutex_lock=PTHREAD_MUTEX_INITIALIZER;
protected:

private:   
    std::string filename;
    int hz;
    std::thread loop_thread;

    std::vector<double> joint_feedforward_tau[TAIL_MOTOR_NUM];
    std::vector<double> joint_position[TAIL_MOTOR_NUM];
    std::vector<double> joint_velocity[TAIL_MOTOR_NUM];

    int direction_motor[TAIL_MOTOR_NUM] = {-1, -1, 1}; //运动学正方向与电机正方向的系数关系
    int direction_limit[TAIL_MOTOR_NUM] = {1, 1, 1};  //两个电机均为正方向限位
    double offset_motor[TAIL_MOTOR_NUM] = {0, 0, 0};   //起始偏移

    double pos_hold[TAIL_MOTOR_NUM];
};

#endif